#include "BspM2_Driver.h"
#include "BspDelay.h"

#define M2BSP_CS(X) X ? (GPIOC->BSRRL = GPIO_Pin_4) : (GPIOC->BSRRH = GPIO_Pin_4) // M2CS------------------PC4

#define M2BSP_DEN(X) X ? (GPIOB->BSRRL = GPIO_Pin_10) : (GPIOB->BSRRH = GPIO_Pin_10) // M2DEN------------------PB10

#define M2BSP_NRST(X) X ? (GPIOC->BSRRL = GPIO_Pin_5) : (GPIOC->BSRRH = GPIO_Pin_5) // M2NRST------------------PC5

static void M2_NFREEZE(uint8_t reset) /*motor halt low active*/
{
	if(reset)
		GPIO_ResetBits(GPIOB,GPIO_Pin_0);
	else
		GPIO_SetBits(GPIOB,GPIO_Pin_0);
}
TypeDefTMC4361_RegStruct TMC4361_M2RegStruct={
	.bsp_write_reg = M2_HardWareSendDat,
	.bsp_read_reg = M2_HardWareReadDat,
	.PROTO_V = 805500,/* 转速*895 这个值大概900转*/
	.bsp_nfreeze = M2_NFREEZE,
	.speed_home = 10,/*10是原本复位速度 其他数值按比例算*/
};

/*********************************************************************************************
  * @brief  M2_CS_IO_Config
  * @param  None
  * @retval None
	* @IO UM2_CS--------PC4
  ********************************************************************************************/
static void M2_CS_IO_Config(void)
{
	GPIO_InitTypeDef GPIOX;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC,ENABLE);
	
	GPIOX.GPIO_Pin = GPIO_Pin_4;
	GPIOX.GPIO_Mode = GPIO_Mode_OUT;
	GPIOX.GPIO_OType = GPIO_OType_PP;
	GPIOX.GPIO_PuPd = GPIO_PuPd_UP;
	GPIOX.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_Init(GPIOC,&GPIOX);
}

/*********************************************************************************************
  * @brief  M2_IO_Config
  * @param  None
  * @retval None
	* @IO UM2_DEN--------PB10
  * @IO UM2_NRST--------PC5
  ********************************************************************************************/
static void M2_IO_Config(void)
{
	GPIO_InitTypeDef GPIOX;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC,ENABLE);
	
	GPIOX.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_0;
	GPIOX.GPIO_Mode = GPIO_Mode_OUT;
	GPIOX.GPIO_OType = GPIO_OType_PP;
	GPIOX.GPIO_PuPd = GPIO_PuPd_UP;
	GPIOX.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_Init(GPIOB,&GPIOX);
	
	GPIO_SetBits(GPIOB,GPIO_Pin_0);/*NFREEZE*/
	
	GPIOX.GPIO_Pin = GPIO_Pin_5;
	GPIO_Init(GPIOC,&GPIOX);
}

uint8_t M2_SPI_Config(void)
{
	Bsp_SPI1_Config();
	M2_CS_IO_Config();
	M2_IO_Config();
	M2BSP_DEN(0);
	Delayms(50);  
	M2BSP_NRST(0);
	Delayms(50); 
	M2BSP_NRST(1);
	Delayms(50); 
}
#include "can_moto_protocol.h"
uint8_t M2_HardWareSendDatCheck(uint8_t RegAddr,uint32_t Data)/*写入数据后读取出来确认*/
{
	uint32_t read_out = 0;
	M2_HardWareSendDat( RegAddr, Data);
	M2_HardWareReadDat( RegAddr, &read_out);
	if(Data != read_out)
	{
		M_STATUS_SET(&TMC4361_M2RegStruct.PROTO_STATUS,M_STATUS_SPI_error);
	}
}

uint8_t M2_HardWareSendDat(uint8_t RegAddr,uint32_t Data)
{
	__disable_irq();
	uint8_t Status = 0;
  M2BSP_CS(0);
	BspCs_Delay(CSDELAY_COUNT);
	RegAddr |= 0X80;
	Status = Bsp_SPI1_SendByte(RegAddr);
	Bsp_SPI1_SendByte((Data >> 24) & 0X000000FF);
	Bsp_SPI1_SendByte((Data >> 16) & 0X000000FF);
	Bsp_SPI1_SendByte((Data >> 8) & 0X000000FF);
	Bsp_SPI1_SendByte(Data & 0X000000FF);
	M2BSP_CS(1);
	BspCs_Delay(CSDELAY_COUNT);
	__enable_irq();
	return Status;
}

uint8_t M2_HardWareReadDat(uint8_t RegAddr,uint32_t *Data)
{
	__disable_irq();
	uint8_t Status = 0;
	M2BSP_CS(0);
	BspCs_Delay(CSDELAY_COUNT);
	Bsp_SPI1_SendByte(RegAddr);
	Bsp_SPI1_SendByte(0);
	Bsp_SPI1_SendByte(0);
	Bsp_SPI1_SendByte(0);
	Bsp_SPI1_SendByte(0);
	M2BSP_CS(1);
	BspCs_Delay(CSDELAY_COUNT);
	
	M2BSP_CS(0);
	BspCs_Delay(CSDELAY_COUNT);
	Status = Bsp_SPI1_SendByte(RegAddr);
	*Data = Bsp_SPI1_SendByte(0);
	(*Data) <<= 8;
	*Data |= Bsp_SPI1_SendByte(0);
	(*Data) <<= 8;
	*Data |= Bsp_SPI1_SendByte(0);
	(*Data) <<= 8;
	*Data |= Bsp_SPI1_SendByte(0);
	M2BSP_CS(1);
	BspCs_Delay(CSDELAY_COUNT);
	__enable_irq();
	return Status;
}

void M2_Init(void)
{
	M2_SPI_Config();
	
	
	M2_HardWareSendDat(0x4F,0x52535400); //reset
	Delayms(100);
	//general setup:
	//----------------------
	M2_HardWareSendDat(0X03,0X540022);      // Inputfilter: START und Encoderpins   输入滤波器配置
	M2_HardWareSendDat(0X04,0X8440000d);   //Setup of SPI Output Timing  SPI输出配置寄存器
	M2_HardWareSendDat(0X00,0x6026);       // direct-a, direct-bow  通用配置寄存器
	M2_HardWareSendDat(0X10,0X60004);       // step length   步进方向输出配置寄存器
	//configuration of TMC2130:   2130 配置寄存器
	//----------------------
	
	M2_HardWareSendDat(0X6D,0X81);      // GCONF
	M2_HardWareSendDat(0X6C,0X07);       // diag1=index, pushpull, direct_mode = on --> SPI mode
	M2_HardWareSendDat(0X6D,0X80);      // GCONF
	M2_HardWareSendDat(0X6C,0X12200);       // diag1=index, pushpull, direct_mode = on --> SPI mode
	M2_HardWareSendDat(0X6D,0X90);      // IHOLD_IRUN
	M2_HardWareSendDat(0X6C,0X00070C08);        // bit 4:0 ihold=0,bit 19:16 iholddelay=7,  bit 12:8 irun=2
	M2_HardWareSendDat(0X6D,0X91);     // TPOWERDOWN
	M2_HardWareSendDat(0X6C,0Xa);           // TZEROWAIT=10
	M2_HardWareSendDat(0X6D,0X94);      // TCOOLTHRS
	M2_HardWareSendDat(0X6C,0Xc8);          // =200
	M2_HardWareSendDat(0X6D,0X95);      // THIGH
	M2_HardWareSendDat(0X6C,0X32);          // =50
	M2_HardWareSendDat(0X6D,0Xee);      // DCCTRL
	M2_HardWareSendDat(0X6C,0X70025);       // DCCTRLSG=5, DC_TIME=37
	M2_HardWareSendDat(0X6D,0Xf0);      // PWMCONF
	M2_HardWareSendDat(0X6C,0X404ff);       // 
	M2_HardWareSendDat(0X6D,0Xec);      // CHOPCONF
	M2_HardWareSendDat(0X6C,0X10223);       //
	M2_HardWareSendDat(0X6D,0Xed);      // COOLCONF (off)
	M2_HardWareSendDat(0X6C,0X100a220);     //
	M2_HardWareSendDat(0X6D,0X8B);      // GLOBAL_SCALER (off)
	M2_HardWareSendDat(0X6C,0X0);   //          global scaler = 0

	//清零速度位置目标位置
	M2_HardWareSendDat(0X24,0X0);  // v=0      VMAX  
	M2_HardWareSendDat(0X21,0X0);  // xact=0   XACTUAL
	M2_HardWareSendDat(0X37,0X0);  // xtarg=0  XTARGET
	M2_HardWareSendDat(0X50,0);    // enc_pos

	//cl basic parameter  闭环基础参数配置
	//----------------------
	M2_HardWareSendDat(0X54,4000);        // encoder resolution = 40k/rev ADAPT THIS PARAMETER to SETUP   编码器分辨率配置寄存器
	M2_HardWareSendDat(0X1C,0Xff00ff);      // CL_BETA=CL_GAMMA=255  闭环调节
	M2_HardWareSendDat(0X5C,0X10000);      // CL_DELTA_P = 1.0    PID_D

	//cl calibration:  闭环校准
	//----------------------
	M2_HardWareSendDat(0X20,0X6);       // ramp mode: hold+pos mode     运行模式
	M2_HardWareSendDat(0X24,0X100000);      // slow vel cl calibration: vmax
	
	M2_HardWareSendDat(0X07,0X400000);      // regulation_modus: turn on cl  编码器信号配置寄存器
//	M2_HardWareSendDat(0X37,0X80);      // move to FS position      目标位置寄存器
	M2_HardWareSendDat(0X07,0X1400000);     // cl_calibration_en: turn on cl calibration    编码器信号配置寄存器
	Delayms(20);     //  延时10MS
	M2_HardWareSendDat(0X07,0X400000);      // cl_calibration_en: turn off cl calibration   编码器配置寄存器
	M2_HardWareSendDat(0X24,0X0);       // v=0    VMAX

	//S-shaped Ramps setup:  S 加减速参数配置
	//----------------------
	M2_HardWareSendDat(0X20,0X6);       // ramp mode: hold+pos mode     运行模式
	M2_HardWareSendDat(0X24,0X05350000);       // vmax=400kpps    VMAX
	M2_HardWareSendDat(0X20,0X06);       // s-ramp + pos mode     运行模式

	//ramp generator    S 曲线斜坡发生器参数
	M2_HardWareSendDat(0X28,10000);     // amax
	M2_HardWareSendDat(0X29,10000);     // dmax 
	M2_HardWareSendDat(0X2D,0x100);     // bow1
	M2_HardWareSendDat(0X2E,0X100);     // bow2
	M2_HardWareSendDat(0X2F,0X100);     // bow3
	M2_HardWareSendDat(0X30,0x100);     // bow4

	//cl operation setup:     闭环设置
	//----------------------
	// back-emf Compensation   反电动势补偿设置
	M2_HardWareSendDat(0X60,250000);       // emf_vmin=
	M2_HardWareSendDat(0X61,450000);       // emf_vadd= -> emf_vmax= 
	M2_HardWareSendDat(0X62,0Xffffff);      // emc_vel0_timer (enc_vel_zero, clock cycles)   编码器延迟时间
	M2_HardWareSendDat(0X63,0X2000864);     // enc vel filter settings (v_enc_mean)   编码器速度配置寄存器
									
	// position Deviation
	M2_HardWareSendDat(0X5F,5);      // cl_tolerance=20     PID 参数寄存器
	M2_HardWareSendDat(0X5C,0X10000);       // cl_p=1.0     PID_D
									
	// catch up velocity   追赶速度参数设置
	M2_HardWareSendDat(0X5A,200);      // cl_vlimit_p=200     PID_P
	M2_HardWareSendDat(0X5B,50);      // cl_vlimit_i=50       PID_I
	M2_HardWareSendDat(0X5D,1000);     // cl_vlimit_diclip=1000   PID_D
	M2_HardWareSendDat(0X5E,200000);       // cl_vlimit=100kpps         PID限幅寄存器

	// closed loop scaling   闭环电流调节参数设置
	M2_HardWareSendDat(0X06,0x32ff64);      // cl scaling values   电流调节寄存器
	M2_HardWareSendDat(0X18,1000);     // cl_upscale=    电流延迟周期寄存器
	M2_HardWareSendDat(0X19,100000);       // cl_dnscale=  电流递减延迟周期寄存器
	M2_HardWareSendDat(0X07,0X0A400000);        // cl encoder signal configuration with gamma correction and vlimit (catch up velocity)     编码器信号配置寄存器

	M2_HardWareSendDat(0X05,0X80);      // cl scaling on  电流调节配置寄存器
	M2_HardWareSendDat(0X62,50000);      // 设置编码器速度更新时钟周期数
	M2_HardWareSendDat(0X53,500);      // ENC_POS_DEV_TOL 设置编码器ENC_POS_DEV 的最大容差值
	M2_HardWareSendDat(0X52,256);				//这里不写的话 TARGET_REACHED不会触发
//	M2_HardWareReadDat(0X0E,000);
	Delayms(10);

}
